#include "../include/HMD.h"

WiimoteConnector* WiiCon = NULL;
int pick_radius = 10.0;

void InitWiimote(void)
{
	if(WiiCon == NULL) {
		WiiCon = new WiimoteConnector();
		Lookat_base_x->set_float_val(lookat[0].x);
		Lookat_base_y->set_float_val(lookat[0].y);
		Lookat_base_z->set_float_val(lookat[0].z);
		Eye_base_x->set_float_val(eye[0].x);
		Eye_base_y->set_float_val(eye[0].y);
		Eye_base_z->set_float_val(eye[0].z);
		
	}
}


void CheckMovement(void)
{
	WiiCon->Update();
	if(WiiCon->Connected(0)) {
		HandleTilt();
		eye[0] = WiiCon->GetEye(0);
		lookat[0] = WiiCon->GetLookat(0);
		wiimote1_connected->set_int_val(1);
		//LED1_x_spin->set_float_val(WiiCon->myWiimote.IR.Dot[0].RawX);
		//LED1_y_spin->set_float_val(WiiCon->myWiimote.IR.Dot[0].RawY);
		//LED2_x_spin->set_float_val(WiiCon->myWiimote.IR.Dot[1].RawX);
		//LED2_y_spin->set_float_val(WiiCon->myWiimote.IR.Dot[1].RawY);
		LED1_1->set_int_val(WiiCon->SeesLED(0, 0));
		LED1_2->set_int_val(WiiCon->SeesLED(0, 1));
		LED1_3->set_int_val(WiiCon->SeesLED(0, 2));
		LED1_4->set_int_val(WiiCon->SeesLED(0, 3));
	}
	else
	{
		wiimote1_connected->set_int_val(0);
	}
	if(WiiCon->Connected(1)) {
		HandleTilt();
		eye[1] = WiiCon->GetEye(1);
		lookat[1] = WiiCon->GetLookat(1);
		wiimote2_connected->set_int_val(1);
		LED2_1->set_int_val(WiiCon->SeesLED(1, 0));
		LED2_2->set_int_val(WiiCon->SeesLED(1, 1));
		LED2_3->set_int_val(WiiCon->SeesLED(1, 2));
		LED2_4->set_int_val(WiiCon->SeesLED(1, 3));
	}
	else
	{
		wiimote2_connected->set_int_val(0);
	}

	//find if the hand is selecting something
	if(live_pick_type == 0) {
		//gogo
		pick_radius = 10.0;
		Point hand = Point((double)eye[1].x, (double)eye[1].y, (double)eye[1].z);
		Point o1 = Point(object_locations[0]);
		Point o2 = Point(object_locations[1]);
		Point o3 = Point(object_locations[2]);
		
		Vector d1 = hand - o1;
		Vector d2 = hand - o2;
		Vector d3 = hand - o3;
		if(WiiCon->ButtonPressed(1, WIIMOTE_BUTTON_A))
		{
			live_dist[0] = d1.length();
			live_dist[1] = d2.length();
			live_dist[2] = d3.length();

			if(live_dist[0] < pick_radius && live_dist[0] < live_dist[1] && live_dist[0] < live_dist[2])
				selected_object = 1;
			if(live_dist[1] < pick_radius && live_dist[1] < live_dist[0] && live_dist[1] < live_dist[2])
				selected_object = 2;
			if(live_dist[2] < pick_radius && live_dist[2] < live_dist[0] && live_dist[2] < live_dist[1])
				selected_object = 3;
			if(live_dist[0] > pick_radius && live_dist[1] > pick_radius && live_dist[2] > pick_radius)
				selected_object = 0;
		} else
			selected_object = 0;
	}
	else if(live_pick_type == 1) {
		//head ray
		pick_radius = 2.0;
		Point hand = Point(eye[1]);
		Point head = Point(eye[0]);
		Vector to_hand = head - hand;
		to_hand.normalize();
		Ray head_ray = Ray(head, to_hand);
		
		double least = DBL_MAX;
		for(int i = 0; i < 3; i ++) {
			Point object = Point(object_locations[i]);
			Vector O(head_ray.origin() - object);
			O.normalize();
			Vector V(head_ray.direction());
			V.normalize();
			double b = Dot(O, V);
			double c = Dot(O, O) - pick_radius * pick_radius;
			float disc = b*b-c;
			live_dist[i] = disc;
			if(disc > 0){
				double sdisc = sqrt(disc);
				double root1 = (-b - sdisc);
				if(root1 > EPSILON && root1 < least)
				{
					selected_object = i + 1;
					least = root1;
					live_dist[i] = root1;
				} else {
					double root2 = (-b + sdisc);
					if(root2 > EPSILON && root2 < least) {
						selected_object = i + 1;
						least = root2;
						live_dist[i] = root2;
					} else {
						live_dist[i] = 0;
					}
				}
			}
		}
		if(least == DBL_MAX) {
			selected_object = 0;
		}
	}
	else if(live_pick_type == 2) {
		//lookat ray
		pick_radius = 1.5;
		Point hand = Point(eye[1]);
		Point lookat_p = Point(lookat[1]);
		Vector to_object = lookat_p - hand;
		to_object.normalize();
		Ray head_ray = Ray(hand, to_object);
		
		double least = DBL_MAX;
		for(int i = 0; i < 3; i ++) {
			Point object = Point(object_locations[i]);
			Vector O(head_ray.origin() - object);
			O.normalize();
			Vector V(head_ray.direction());
			V.normalize();
			double b = Dot(O, V);
			double c = Dot(O, O) - pick_radius * pick_radius;
			float disc = b*b-c;
			live_dist[i] = disc;
			if(disc > 0){
				double sdisc = sqrt(disc);
				double root1 = (-b - sdisc);
				if(root1 > EPSILON && root1 < least)
				{
					selected_object = i + 1;
					least = root1;
					live_dist[i] = root1;
				} else {
					double root2 = (-b + sdisc);
					if(root2 > EPSILON && root2 < least) {
						selected_object = i + 1;
						least = root2;
						live_dist[i] = root2;
					} else {
						live_dist[i] = 0;
					}
				}
			}
		}
		if(least == DBL_MAX) {
			selected_object = 0;
		}
	}
}

void HandleTilt(void)
{
	float pitch = WiiCon->GetPitch(0);
	float roll = WiiCon->GetRoll(0);
	Pitch->set_float_val(pitch);
	Roll->set_float_val(roll);
	//use a base lookat point of 0,0,0 if the pitch is 0
	//float pitch_radians = deg_to_rad(pitch);
	//lookat[1] = tan(pitch_radians) * eye[2];
	//find the new up
	float roll_prime_radians;
	if(roll > 0) {
		roll_prime_radians = deg_to_rad(90 - roll);
		up[0].x = -cos(roll_prime_radians);
		up[0].y = sin(roll_prime_radians);
		up[0].z = 0;
	}
	else {
		roll_prime_radians = deg_to_rad(90 + roll);
		up[0].x = cos(roll_prime_radians);
		up[0].y = sin(roll_prime_radians);
		up[0].z = 0;
	}

}
